#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Odometry.h>
#include <tf/tfMessage.h>
#include <tf/transform_listener.h>

geometry_msgs::Pose pose_msg;

void poseCallback(const nav_msgs::Odometry::ConstPtr& msg) 
{    
    pose_msg.position.x = msg->pose.pose.position.x;
    pose_msg.position.y = msg->pose.pose.position.y;
    pose_msg.position.z = msg->pose.pose.position.z;
    pose_msg.orientation.x = msg->pose.pose.orientation.x;
    pose_msg.orientation.y = msg->pose.pose.orientation.y;
    pose_msg.orientation.z = msg->pose.pose.orientation.z;
    pose_msg.orientation.w = msg->pose.pose.orientation.w;
}

int main(int argc, char *argv[]) {

    ros::init(argc, argv, "location_gazebo");
    ros::NodeHandle nh;


    ros::Publisher pub = nh.advertise<geometry_msgs::Pose>("/location", 100);

    ros::Subscriber sub = nh.subscribe<nav_msgs::Odometry>("/car/car_state", 10000, poseCallback);

    while(ros::ok) {
        ros::spinOnce(); 
	std::cout<<"pose:x y "<<pose_msg.position.x<<" "<<pose_msg.position.y<<std::endl;
        pub.publish(pose_msg); // 发布位姿信息
    }
    return 0;
}
